#! /usr/bin/env python3


import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped
from tf2_ros import StaticTransformBroadcaster

if __name__ == "__main__":
    rospy.init_node("static_transform_publisher")
    #创建广播的发布者
    broadcaster = StaticTransformBroadcaster()
    
    #创建相关坐标系
    tfs1=TransformStamped()
    tfs1.header.frame_id="world"
    tfs1.header.stamp=rospy.Time.now()
    tfs1.header.seq=101
    tfs1.child_frame_id="son1"
    tfs1.transform.translation.x=1.0
    tfs1.transform.translation.y=0.0
    tfs1.transform.translation.z=0.0
    qtn=tf.transformations.quaternion_from_euler(0,0,0)
    tfs1.transform.rotation.x=qtn[0]
    tfs1.transform.rotation.y=qtn[1]
    tfs1.transform.rotation.z=qtn[2]
    tfs1.transform.rotation.w=qtn[3]

    broadcaster.sendTransform(tfs1)
    print("success sendTransform tfs1")
    
    # spin
    rospy.spin()

